#include <rtthread.h>
#include "./Avoid.h"
#include "./Modbus/RTU/Master/modbusrtu_master.h"

#define LOG_TAG "avoid"     /* 该模块TAG */
#define LOG_LVL LOG_LVL_DBG /* 静态过滤级别为调试级 */
#include <ulog.h>           /* 必须放在宏定义下面 */

static void main_thread(void *parameter);

static struct rt_thread _thread;
ALIGN(RT_ALIGN_SIZE)
static char _thread_stack[1024];

static enum Sensor_Enum avoid[4]; /* 0~3对应前后左右4个避障 */
static unsigned short avoid_temp[6];
static unsigned short emergency[2];

static struct rt_event _event;
static unsigned short pbs_area = 0;
static char slave_addr;

static void pbs_thread(void *parameter);

void Avoid_Init(char addr)
{
    rt_err_t rt_result;
    slave_addr = addr;
    rt_event_init(&_event, "avoid event", RT_IPC_FLAG_PRIO);
    rt_result = rt_thread_init(&_thread, "avoid thread", main_thread, RT_NULL, _thread_stack, sizeof(_thread_stack), 5, 2);
    if (rt_result == RT_EOK)
    {
        rt_thread_startup(&_thread); /* 启动线程 */
    }
    else
    {
        rt_kprintf("avoid thread error\n");
    }

    struct rt_thread *temp = rt_thread_create("pbs set area", pbs_thread, RT_NULL, 512, 10, 2);
    if (temp != RT_NULL)
    {
        rt_thread_startup(temp); /* 启动线程 */
    }
}

enum Sensor_Enum Avoid_Read(int index)
{
    index %= 4;
    return avoid[index];
}

void main_thread(void *parameter)
{
    rt_err_t rt_result;
    rt_uint32_t event_value;
    Modbus_Error_Code mb_result;

    while (1)
    {
        rt_result = rt_event_recv(&_event, 1 << 0, RT_EVENT_FLAG_CLEAR | RT_EVENT_FLAG_OR, 50, &event_value);
        if (RT_EOK == rt_result)
        {
            mb_result = MR_Writer_Holding_Register(slave_addr, 0x0013, pbs_area, RT_WAITING_FOREVER); /* 设置PBS区域 */
            if (Modbus_Master_OK != mb_result)
            {
                rt_event_send(&_event, 1 << 0);
            }
        }
        MR_Read_Holding_Registers(slave_addr, 0x0000, 6, avoid_temp, RT_WAITING_FOREVER);
        for (int i = 0; i < 4; i++)
        {
            avoid[i] = (enum Sensor_Enum)avoid_temp[i];
        }
        for (int i = 0; i < 2; i++)
        {
            emergency[i] = avoid_temp[i + 4];
        }
    }
}

void Avoid_Set_Area(char area)
{
    pbs_area = area;
    rt_event_send(&_event, 1 << 0);
}

unsigned short Return_Avoid_Area(void)
{
    return pbs_area;
}

void pbs_thread(void *parameter)
{
    rt_thread_delay(3000);
    Avoid_Set_Area(15);
}

unsigned short Read_Emergency(int index)
{
    if (index < 2)
    {
        return emergency[index];
    }
    else
    {
        return 0;
    }
}
